/*
 * Copyright 2016-2020 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef WAY_PLANNER_CORE_H_
#define WAY_PLANNER_CORE_H_

#include <ros/ros.h>

#include "vector_map_msgs/PointArray.h"
#include "vector_map_msgs/LaneArray.h"
#include "vector_map_msgs/NodeArray.h"
#include "vector_map_msgs/StopLineArray.h"
#include "vector_map_msgs/DTLaneArray.h"
#include "vector_map_msgs/LineArray.h"
#include "vector_map_msgs/AreaArray.h"
#include "vector_map_msgs/SignalArray.h"
#include "vector_map_msgs/StopLine.h"
#include "vector_map_msgs/VectorArray.h"

#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>

#include <std_msgs/Int8.h>
#include "libwaypoint_follower/libwaypoint_follower.h"
#include "autoware_can_msgs/CANInfo.h"
#include <visualization_msgs/MarkerArray.h>

#include "op_planner/MappingHelpers.h"
#include "op_planner/PlanningHelpers.h"
#include "op_planner/PlannerH.h"
#include "ROSHelpers.h"
#include "SocketServer.h"

#include <string>
#include <vector>

namespace WayPlannerNS
{

#define MAX_GLOBAL_PLAN_DISTANCE 10000
#define ENABLE_VISUALIZE_PLAN
#define REPLANNING_DISTANCE 25

class AutowareRoadNetwork
{
  public:
    vector_map_msgs::PointArray    points;
    vector_map_msgs::LaneArray     lanes;
    vector_map_msgs::NodeArray     nodes;
    vector_map_msgs::StopLineArray stoplines;
    vector_map_msgs::DTLaneArray   dtlanes;  // center lines
    vector_map_msgs::AreaArray     areas;
    vector_map_msgs::LineArray     lines;
    vector_map_msgs::SignalArray   signals;
    vector_map_msgs::VectorArray   vectors;

    bool bPoints;
    bool bLanes;
    bool bNodes;
    bool bStopLines;
    bool bDtLanes;
    bool bAreas;
    bool bLines;
    bool bSignals;
    bool bVectors;

    AutowareRoadNetwork()
    {
      bPoints    = false;
      bLanes     = false;
      bStopLines = false;
      bDtLanes   = false;
      bNodes     = false;
      bAreas     = false;
      bLines     = false;
      bSignals   = false;
      bVectors   = false;
    }
};

enum MAP_SOURCE_TYPE{MAP_AUTOWARE, MAP_FOLDER, MAP_KML_FILE};

class WayPlannerParams
{
  public:
    std::string      KmlMapPath;
    bool             bEnableSmoothing;
    bool             bEnableLaneChange;
    bool             bEnableHMI;
    bool             bEnableRvizInput;
    bool             bEnableReplanning;
    double           pathDensity;
    double           fallbackMinGoalDistanceTh;
    int              planningMaxAttempt;
    MAP_SOURCE_TYPE  mapSource;


    WayPlannerParams()
    {
      bEnableReplanning         = false;
      bEnableHMI                = false;
      bEnableSmoothing          = false;
      bEnableLaneChange         = false;
      bEnableRvizInput          = true;
      pathDensity               = 0.5;
      fallbackMinGoalDistanceTh = 0.0;
      planningMaxAttempt        = 0;
      mapSource                 = MAP_KML_FILE;
    }
};

class way_planner_core
{
  protected:
    WayPlannerParams m_params;
    AutowareRoadNetwork m_AwMap;
    PlannerHNS::WayPoint m_CurrentPose;
    int m_iCurrentGoalIndex;
    std::vector<PlannerHNS::WayPoint> m_GoalsPos;
    geometry_msgs::Pose m_OriginPos;
    PlannerHNS::VehicleState m_VehicleState;

    std::vector<geometry_msgs::PoseStamped> m_NodesList;

    ros::NodeHandle nh;

    ros::Publisher pub_MapRviz;
    ros::Publisher pub_Paths;
    ros::Publisher pub_PathsRviz;
    ros::Publisher pub_TrafficInfo;
    ros::Publisher pub_TrafficInfoRviz;
    ros::Publisher pub_StartPointRviz;
    ros::Publisher pub_GoalPointRviz;
    ros::Publisher pub_NodesListRviz;

    ros::Subscriber sub_robot_odom;
    ros::Subscriber sub_start_pose;
    ros::Subscriber sub_goal_pose;
    ros::Subscriber sub_current_pose;
    ros::Subscriber sub_current_velocity;
    ros::Subscriber sub_nodes_list;
    ros::Subscriber sub_map_points;
    ros::Subscriber sub_map_lanes;
    ros::Subscriber sub_map_nodes;
    ros::Subscriber sup_stop_lines;
    ros::Subscriber sub_dtlanes;
    ros::Subscriber sub_can_info;

  public:
    way_planner_core();
    ~way_planner_core();
    void PlannerMainLoop();

  private:
    void GetTransformFromTF(const std::string parent_frame, const std::string child_frame,
            tf::StampedTransform &transform);

    // Callback function for subscriber.
    void callbackGetGoalPose(const geometry_msgs::PoseStampedConstPtr &msg);
    void callbackGetStartPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &input);
    void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
    void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg);
    void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg);
    void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg);

    void callbackGetVMPoints(const vector_map_msgs::PointArray& msg);
    void callbackGetVMLanes(const vector_map_msgs::LaneArray& msg);
    void callbackGetVMNodes(const vector_map_msgs::NodeArray& msg);
    void callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg);
    void callbackGetVMCenterLines(const vector_map_msgs::DTLaneArray& msg);
    void callbackGetNodesList(const vector_map_msgs::NodeArray& msg);

    protected:
      PlannerHNS::RoadNetwork m_Map;
      bool  m_bKmlMap;
      PlannerHNS::PlannerH m_PlannerH;
      std::vector<std::vector<PlannerHNS::WayPoint>> m_GeneratedTotalPaths;

      void UpdateRoadMap(const AutowareRoadNetwork& src_map, PlannerHNS::RoadNetwork& out_map);
      bool GenerateGlobalPlan(PlannerHNS::WayPoint& startPoint, PlannerHNS::WayPoint& goalPoint,
              std::vector<std::vector<PlannerHNS::WayPoint>>& generatedTotalPaths);
      void VisualizeAndSend(const std::vector<std::vector<PlannerHNS::WayPoint>> generatedTotalPaths);

    private:  // debug variables
      PlannerHNS::WayPoint* m_pCurrGoal;
#ifdef ENABLE_VISUALIZE_PLAN
      ros::Publisher pub_GlobalPlanAnimationRviz;
      void CreateNextPlanningTreeLevelMarker(std::vector<PlannerHNS::WayPoint*>& level,
              visualization_msgs::MarkerArray& markerArray, double max_cost = 1);
      std::vector<PlannerHNS::WayPoint*> m_PlanningVisualizeTree;
      std::vector<PlannerHNS::WayPoint*> m_CurrentLevel;
      visualization_msgs::MarkerArray m_AccumPlanLevels;
      unsigned int m_iCurrLevel;
      unsigned int m_nLevelSize;
      double m_CurrMaxCost;
      int m_bSwitch;
#endif

      double m_AvgResponseTime;  // seconds
      HMISocketServer m_SocketServer;
      std::vector<PlannerHNS::WayPoint*> m_ModifiedWayPointsCosts;
      bool HMI_DoOneStep();
};

}  // namespace WayPlannerNS

#endif  // WAY_PLANNER_CORE_H_
